#include <msp430x16x.h>
#include "sd.h"

/* place to hold pointers to sd memory locations

   sdvars[0] is the location of the current sector being written to in the log section
   sdvars[1] is the location of the current sector being written to in the image section

   the log section is from 2mb to 1024mb and can contain up to 2,093,056 (0x1ff000) 512 sectors
   this is only expressable in a long int, which wastes too much memory, so three char are used
   to represent this

   the total offset is given by top*mid*btm
 */
/* useful defines to make code more meaningful */
#define R1 1
#define R1B 2
#define R2 3
#define R3 4
/* card reset */
#define CMD0 0
#define CMD0_R R1
/* set block length */
#define CMD16 16
#define CMD16_R R1
/* read block */
#define CMD17 17
#define CMD17_R R1
/* write block */
#define CMD24 24
#define CMD24_R R1
/* Get card OCR (sd mode) */
#define ACMD41 41
#define ACMD41_R R1
/* start app specific */
#define CMD55 55
#define CMD55_R R1
/* Read the OCR (spi mode) */
#define CMD58 58
#define CMD58_R R3
/* macros */
#define SPIASSERT P3OUT &= ~0x1 /* macro'd due to being a single command rather than a function call */
#define SPIDEASSERT P3OUT |= 0x1 /* macro'd due to being a single command rather than a function call */
#define SDPACKARG(a,v) a[3] = (unsigned char)(v >> 24); a[2] = (unsigned char)(v >> 16);\
                       a[1] = (unsigned char)(v >> 8);a[0] = (unsigned char)(v) /* macro'd as only called a few times */

/* internal functions */
static void SPISendByte(unsigned char c);
static unsigned char SPIReadByte(void);
static int sd_send_command(unsigned char cmd, unsigned char response_type);
static void sd_delay(char number);
unsigned char sd_response[5];
unsigned char sd_argument[4];
#ifndef SD_BA
static int sd_set_blocklen(unsigned long int length);
static int sd_read_block_complex(unsigned long int blockaddr,unsigned int offset,unsigned int size);
static int sd_write_block(unsigned long int blockaddr);
union __attribute__ ((packed)){
  unsigned char b[4]; /* top is [2], mid is [1], btm is [0] */
  unsigned long l;
}sdvarlog,sdvarimg;
#define SDHashLocation 25 /* location in bytes of the record hash */
#define SDRecordSize 32 /* size in bytes of record */
#define SDMaxRecord 15 /* maximum number of sd records per sector */
typedef struct __attribute__ ((packed)){
  char recordNumber;
  _sd_record sd_record[15];
  char __padding[31];
}_sd_sector;
union __attribute__ ((packed)){
  unsigned char r[512];
  _sd_sector s;
}sd_buffer;
#endif /* SD_BA */

/* functions */
static void SPISendByte(unsigned char c)
{
  IFG1 &= ~URXIFG0;
  TXBUF0=c;
  while ((IFG1 & URXIFG0) == 0) { }
}
static unsigned char SPIReadByte(void)
{
  unsigned char tmp;
  IFG1 &= ~URXIFG0;
  TXBUF0 = 0xff; /* need to push a byte out to recieve one back in */
  while ((IFG1 & URXIFG0) == 0) { }
  tmp = U0RXBUF;
  return (tmp);
}
int sdInit(void)
{
  char i;
  #ifndef SD_BA
  /* argument and response to zero */
  sd_argument[0]=0;sd_argument[1]=0;sd_argument[2]=0;sd_argument[3]=0;
  sd_response[0]=0;sd_response[1]=0;sd_response[2]=0;sd_response[3]=0;sd_response[4]=0;
  /* sd vars to high, padding to 0 */
  sdvarlog.b[2]=0xff;sdvarlog.b[1]=0xff;sdvarlog.b[0]=0xff;sdvarlog.b[3]=0;
  sdvarimg.b[2]=0xff;sdvarimg.b[1]=0xff;sdvarimg.b[0]=0xff;sdvarimg.b[3]=0;
  #endif /* SD_BA */

  /* enable power */
  P4SEL|=0x20; /* select pin 5 of port 4 */
  P4DIR|=0x20; /* select pin 5 of port 4 as an output */
  P4OUT|=0x20;

  /************************************************************************************************
   * SPI / USART init
   ***********************************************************************************************/
  P3SEL |= 0xe; /* set the p3 pins we are using to spi */
  P3OUT |= 0x1; /* chip select high for inactive */
  P3DIR |= 0x1; /* set pin 0 as an output */
  U0CTL = (CHAR | SYNC | MM | SWRST); /* 8-bit spi with us as master */
  U0TCTL = (SSEL1 | STC | CKPH); /* normal 3 wire spi */
  U0BR0 = 0x14; /* start the clock at a low speed for mmc compatibility */
  U0BR1 = 0x0; /* must be zero */
  U0MCTL = 0x0; /* so must this one */
  ME1 |= USPIE0; /* enable the spi module */
  U0CTL &= ~SWRST; /* remove usart from software reset */

  /************************************************************************************************
   * SD init
   ***********************************************************************************************/
  /* delay for at least 74 clock cycles */
  SPIASSERT;
  sd_delay(100);
  SPIDEASSERT;
  sd_delay(2);
  /* set the card to idle */
  if (sd_send_command(CMD0, CMD0_R) == 0)
    return 0;
  /* Now wait until the card goes idle. Retry at most SD_IDLE_WAIT_MAX (100) times */
  i = 0;
  do
  {
    i++;
    /* Flag the next command as an application-specific command */
    if (sd_send_command(CMD55, CMD55_R) == 1)
    {
      /* Tell the card to send its OCR */
      sd_send_command(ACMD41, ACMD41_R);
    }
    else
    {
      /* No response, bail early */
      i = 100;
    }
  }
  while ((sd_response[0] & 0x1) == 0x1 && i < 100);
  /* As long as we didn't hit the timeout, assume we're OK. */
  if (i >= 100)
    return 0;
  if (sd_send_command(CMD58, CMD58_R) == 0)
    return 0;
  /* At a very minimum, we must allow 3.3V. */
  if ((sd_response[2] & 0xc0) != 0xc0)
    return 0;
  /* Set the block length */
  if (sd_set_blocklen (512) != 1)
    return 0;
  /* If we got this far, initialization was okay. */
  /* we can also increase the clock speed of the usart back to something more useful */
  U0CTL |= SWRST;
  U0BR0 = 2;
  U0CTL &= ~SWRST;
  return 1;
}
static int sd_send_command(unsigned char cmd, unsigned char response_type)
{
  int i;
  char response_length;
  unsigned char tmp;
  SPIASSERT;
  /* All data is sent MSB first, and MSb first */
  /* Send the header/command */
  /* Format:
  cmd[7:6] : 01
  cmd[5:0] : command */
  SPISendByte((cmd & 0x3F) | 0x40);
  for (i=3; i>=0; i--)
  {
    SPISendByte(sd_argument[i]);
  }
  /* This is the CRC. It only matters what we put here for the first
  command. Otherwise, the CRC is ignored for SPI mode unless we
  enable CRC checking. */
  SPISendByte(0x95);
  response_length = 0;
  switch (response_type)
  {
    case R1:
    case R1B:
      response_length = 1;
      break;
    case R2:
      response_length = 2;
      break;
    case R3:
      response_length = 5;
      break;
    default:
      break;
  }
  /* Wait for a response. A response can be recognized by the
  start bit (a zero) */
  i=0;
  do
  {
    tmp = SPIReadByte();
    i++;
  }
  while (((tmp & 0x80) != 0) && i < 100);
  /* Just bail if we never got a response */
  if (i >= 100)
  {
    SPIDEASSERT;
    return 0;
  }
  for (i=response_length-1; i>=0; i--)
  {
    sd_response[i] = tmp;
    /* This handles the trailing-byte requirement. */
    tmp = SPIReadByte();
  }
  /* If the response is a "busy" type (R1B), then there's some
  * special handling that needs to be done. The card will
  * output a continuous stream of zeros, so the end of the BUSY
  * state is signaled by any nonzero response. The bus idles
  * high.
  */
  i=0;
  if (response_type == R1B)
  {
    do
    {
      i++;
      tmp = SPIReadByte();
    }
    /* This should never time out, unless SDI is grounded.
    * Don't bother forcing a timeout condition here. */
    while (tmp != 0xFF);
    SPISendByte(0xFF);
  }
  SPIDEASSERT;
  return 1;
}
static void sd_delay(char number)
{
  register char i;
  /* Null for now */
  for (i=0; i<number; i++)
  {
    /* Clock out an idle byte (0xFF) */
    SPISendByte(0xFF);
  }
}
void sd_wait_notbusy(void)
{
  /* Just twiddle our thumbs until the transfer's done */
  while ((DMA0CTL & DMAEN) != 0) { }
  /* Reset the DMA controller */
  DMACTL0 = 0;
  /* Ignore the checksum */
  sd_delay(4);
  while (SPIReadByte() != 0xFF);
  /* Deassert CS */
  SPIDEASSERT;
  /* Send some extra clocks so the card can resynchronize on the next
  transfer */
  sd_delay(2);
}
/* Set the block length for all future block transactions */
/* Returns 1 if the function was successful */
#ifndef SD_BA
static int sd_set_blocklen(unsigned long int length)
#else
int sd_set_blocklen(unsigned long int length)
#endif /* SD_BA */
{
  /* Pack the block length */
  SDPACKARG(sd_argument, length);
  return (sd_send_command(CMD16, CMD16_R));
}
#ifdef SD_BA
#define sd_read_block(a,b) sd_read_block_complex(a,0,b,512)
int sd_read_block_complex(unsigned long int blockaddr,unsigned int offset,unsigned char *data,unsigned int size)
#else
#define sd_read_block(a) sd_read_block_complex(a,0,512)
static int sd_read_block_complex(unsigned long int blockaddr,unsigned int offset,unsigned int size)
#endif /* SD_BA */
{
  unsigned long int i = 0;
  unsigned char tmp;
  const unsigned char blank = 0xFF; /* const so it is moved into .text section */
  /* Adjust the block address to a linear address by multiplying by 512 */
  blockaddr <<= 9;
  /* Wait until any old transfers are finished */
  sd_wait_notbusy();
  /* Pack the address */
  SDPACKARG(sd_argument, blockaddr);
  /* Need to add size checking */
  if (sd_send_command(CMD17, CMD17_R) == 0)
    return 0;
  /* Check for an error, like a misaligned read */
  if (sd_response[0] != 0)
    return 0;
  /* Re-assert CS to continue the transfer */
  SPIASSERT;
  /* Wait for the token */
  i=0;
  do
  {
    tmp = SPIReadByte();
    i++;
  }
  while ((tmp == 0xFF) && i < 400000 );
  if ((tmp & 0xe0) == 0)
  {
    /* Clock out a byte before returning */
    SPISendByte(0xFF);
    /* The card returned an error response. Bail and return 0 */
    return 0;
  }
  /* Prime the interrupt flags so things happen in the correct order. */
  IFG1 &= ~URXIFG0;
  IFG1 &= ~UTXIFG0;
  /* Get the block */
  /* Source DMA address: receive register. */
  DMA0SA = U0RXBUF_;
  /* Destination DMA address: the user data buffer. */
  #ifdef SD_BA
  DMA0DA = (unsigned short)data;
  #else
  DMA0DA = (unsigned short)sd_buffer.r;
  #endif /* SD_BA */
  /* The size of the block to be transferred */
  DMA0SZ = size;
  /* Configure the DMA transfer*/
  DMA0CTL =
    DMADT_0 | /* Single transfer mode */
    DMASBDB | /* Byte mode */
    DMAEN | /* Enable DMA */
    DMADSTINCR1 | DMADSTINCR0; /* Increment the destination address */
  /* We depend on the DMA priorities here. Both triggers occur at
  the same time, since the source is identical. DMA0 is handled
  first, and retrieves the byte. DMA1 is triggered next, and
  sends the next byte. */
  /* Source DMA address: constant 0xFF (don't increment)*/
  DMA1SA = (unsigned short)&blank;
  /* Destination DMA address: the transmit buffer. */
  DMA1DA = U0TXBUF_;
  /* Increment the destination address */
  /* The size of the block to be transferred */
  DMA1SZ = size-1;
  /* Configure the DMA transfer*/
  DMA1CTL =
    DMADT_0 | /* Single transfer mode */
    DMASBDB | /* Byte mode */
    DMAEN; /* Enable DMA */
  /* DMA trigger is UART receive for both DMA0 and DMA1 */
  DMACTL0 = DMA0TSEL_3 | DMA1TSEL_3;
  /* Kick off the transfer by sending the first byte */
  U0TXBUF = 0xFF;
  return 1;
}
#ifdef SD_BA
int sd_write_block(unsigned long int blockaddr,unsigned char *data)
#else
static int sd_write_block(unsigned long int blockaddr)
#endif /* SD_BA */
{
  /* Adjust the block address to a linear address */
  blockaddr <<= 9;
  /* Wait until any old transfers are finished */
  sd_wait_notbusy();
  /* Pack the address */
  SDPACKARG(sd_argument, blockaddr);
  if (sd_send_command(CMD24, CMD24_R) == 0)
    return 0;
  /* Check for an error, like a misaligned write */
  if (sd_response[0] != 0)
    return 0;
  /* Re-assert CS to continue the transfer */
  SPIASSERT;
  /* The write command needs an additional 8 clock cycles before
  * the block write is started. */
  SPIReadByte();
  /* Clear any pending flags */
  IFG1 &= ~(URXIFG0 | UTXIFG0);
  /* Get the block */
  /* Source DMA address: the data buffer. */
  #ifdef SD_BA
  DMA0SA = (unsigned short)data;
  #else
  DMA0SA = (unsigned short)sd_buffer.r;
  #endif /* SD_BA */
  /* Destination DMA address: the UART send register. */
  DMA0DA = U0TXBUF_;
  /* The size of the block to be transferred */
  DMA0SZ = 512;
  /* Configure the DMA transfer*/
  DMA0CTL =
    DMADT_0 | /* Single transfer mode */
    DMASBDB | /* Byte mode */
    DMAEN | /* Enable DMA */
    DMASRCINCR1 | DMASRCINCR0; /* Increment the source address */
  /* DMA trigger is UART send */
  DMACTL0 = DMA0TSEL_3;
  /* Kick off the transfer by sending the first byte, the "start block"
  * token */
  U0TXBUF = 0xfe;
  return 1;
}
#ifdef SD_STANDALONE
int main()
{
  return 0;
}
#endif /* SD_STANDALONE */

/* interface */
/* sdEnable and sdDisable, quicker to use than sdInit, providing sdInit has already been run */
void sdDisable(void)
{
  /* check that the dma is not using the card */
  sd_wait_notbusy();
  /* disable sd card */
  SPIDEASSERT;
  /* return to orginal usart0 values */
  ME1 &= ~USPIE0; /* disable spi */
  U0CTL = SWRST; /* place usart in to software reset */
  U0TCTL = TXEPT; /* reset transmit register */
  U0BR0 = 0;
  U0BR1 = 0;
  U0MCTL = 0;
  /* disable pins that we used */
  P3SEL &= ~0xe;
  P3OUT &= ~0x1;
  P3DIR &= ~0x1;
}
void sdEnable(void)
{
  /* re-enable pins that we use */
  P3SEL |= 0xe;
  P3OUT |= 0x1;
  P3DIR |= 0x1;
  /* set up usart0 */
  U0CTL = (CHAR | SYNC | MM | SWRST);
  U0TCTL = (SSEL1 | STC | CKPH);
  U0BR0 = 0x2;
  U0BR1 = 0x0;
  U0MCTL = 0x0;
  ME1 |= USPIE0; /* enable the spi module */
  U0CTL &= ~SWRST; /* remove usart from software reset */
  /* enable the sd card */
  SPIASSERT;
}


#ifndef SD_BA
#define sdFlightVar  0
#define sdLogMeta    0x800
  #define sdLogTbl1 0x800
  #define sdLogTbl2 0x801 /* the second log table is in a different sector to that of the first so that they
                             they both can not be destroyed by one single write */
#define sdLog        0x1000
#define sdImgMeta    0x200000
#define sdImg        0x200800
#define sdTop        0x3B9ACA /* should ideally be 0x400000 but isnt due to 2gb sd card being 2E9 bytes */

static void sd_hash_record(char* record)
{
  unsigned int i;
  unsigned int* h=(unsigned int*)(&record[SDHashLocation]);
  for(i=0;i<SDHashLocation;++i)
    (*h)=(((*h)<<5)+(*h))^record[i];
  return;
}

#define sdLogSectorNotValid ((sdvarlog.l >= sdImgMeta) || (sdvarlog.l < sdLog))

int sdPreData(void)
{
  unsigned int i; /* temporary variable for loops */
  /* check to see what sector we should be on, if top, mid and btm are high (0xff) then
     we reload in the data, if the current sector is greater than the last sector, or less than the first sector
     then we also reload the data
   */
  if(((sdvarlog.b[2]==0xff) && (sdvarlog.b[1]==0xff) && (sdvarlog.b[0]==0xff)) || (sdLogSectorNotValid))
  {
    /* we have the start or fault condition
       we will now re-read in the sd card location data
     */
    sd_set_blocklen(3); /* change the block length to 3 bytes */
    sd_read_block_complex(sdLogTbl1,0,3); /* read in the 3 bytes from the location of the first sd log table */
    sd_wait_notbusy();
    /* place the contents of the first sd log table into sdvarlog */
    sdvarlog.b[2]=sd_buffer.r[0];
    sdvarlog.b[1]=sd_buffer.r[1];
    sdvarlog.b[0]=sd_buffer.r[2];
    /* if the sector address is higher than the highest possible sector, or if it is lower than the lowest possible sector */
    if(sdLogSectorNotValid)
    {
      /* we read in the values from the second table */
      sd_read_block_complex(sdLogTbl2,0,3); /* read in first 3 bytes from the second log table */
      sd_wait_notbusy();
      /* put contents into sdvarlog */
      sdvarlog.b[2]=sd_buffer.r[0];
      sdvarlog.b[1]=sd_buffer.r[1];
      sdvarlog.b[0]=sd_buffer.r[2];
      /* again test for the boundary conditions */
      if(sdLogSectorNotValid)
      {
        /* only at this point do we cycle round to the first possible sector
           this could be a good time to raise a log message so we know this has happened */
        sdvarlog.l=sdLog;
        /* we will now clear the sector prior to writing out the two log tables, so if the mcu is reset whilst at
           this point, it will do do the same action again rather than think this sector has just been completed */
        sd_set_blocklen(512);
        for(i=0;i<512;++i) /* cycle from 0 to 511, and set each byte to zero in buffer -----------------------------could remove padding from this blanking */
        {                  /* records in sector cancelled to zero as just reseting record number could affect data in reset situation */
          sd_buffer.r[i]=0;
        }
        sd_write_block(sdvarlog.l);
        sd_wait_notbusy();
        /* now we write out the log tables */
        sd_buffer.r[0]=sdvarlog.b[2];
        sd_buffer.r[1]=sdvarlog.b[1];
        sd_buffer.r[2]=sdvarlog.b[0];
        sd_write_block(sdLogTbl1);
        sd_wait_notbusy();
        sd_write_block(sdLogTbl2);
        sd_wait_notbusy();
      }
      else
      {
        /* to get here, our first log table must be pointing too high or too low, but the second table is valid
           we will therefore just write out the second table to the first location, the second location will be
           in the sd_buffer, so we can just call a write */
        /* change to 512 for writing, sd card limitation */
        sd_set_blocklen(512);
        /* write out block stored in buffer from the second log table position to the first log table position */
        sd_write_block(sdLogTbl1);
        sd_wait_notbusy();
      }
    }
  }
  /* we now need to check that the sector we are about to read in has room to write to */
  /* so we read only the first byte in */
  sd_set_blocklen(1);
  sd_read_block(sdvarlog.l);
  sd_wait_notbusy();
  /* we then compare this to the max record number */
  if(sd_buffer.s.recordNumber < SDMaxRecord)
  {
    /* we have a valid sector that can be read in so we read it in */
    sd_set_blocklen(512);
    sd_read_block(sdvarlog.l);
    /* we do not call sd_wait_notbusy as we can still read in stuff whilst the adcs etc. is collecting */
    return 0;
  }
  else
  {
    /* we need to use the next sector, and therefore increase sdvarlog */
    /* we will first blank out the sector, then write the log tables as this stops us from thinking we've just written this */
    /* increase the sector number */
    ++sdvarlog.l;
    /* now we check to see if this is a valid sector */
    if(sdLogSectorNotValid)
    {
      /* if it isnt, we need to cycle round to the start */
      /*********************************************************************************************************************************************
       * TODO **************************************************************************************************************************************
       ********************************************************************************************************************************************/
    }

    /* once we get to this point, we need to write out a blank sector, then update the log tables*/
    sd_set_blocklen(512);
    for(i=0;i<512;++i) /* cycle from 0 to 511, and set each byte to zero in buffer -----------------------------could remove padding from this blanking */
    {                  /* records in sector cancelled to zero as just reseting record number could affect data in reset situation */
      sd_buffer.r[i]=0;
    }
    sd_write_block(sdvarlog.l);
    sd_wait_notbusy();
    /* now we write out the log tables */
    sd_buffer.r[0]=sdvarlog.b[2];
    sd_buffer.r[1]=sdvarlog.b[1];
    sd_buffer.r[2]=sdvarlog.b[0];
    sd_write_block(sdLogTbl1);
    sd_wait_notbusy();
    sd_write_block(sdLogTbl2);
    sd_wait_notbusy();
    /* we then clear the sdvarlog data from the buffer, ready for sdPostData */
    sd_buffer.r[0]=0;sd_buffer.r[1]=0;sd_buffer.r[2]=0;
  }
  return 0;
}

#endif /* SD_BA */
